clear;clc

%% Filter dominated locking
gain = 1; order=8; bw=2.348;
plant=EquipmentControl.FeedBackOptimization.LTIUnit.StaticGain(gain);
filter=EquipmentControl.FeedBackOptimization.LTIUnit.LowPassFilter(order, bw);
tune = EquipmentControl.FeedBackOptimization.PIDOptimizer(plant, filter);

figure; 
tune.setController1stOrderApprox();
tune.response('axes', subplot(2, 2, 1));

tune.setControllerOptimal('type', 'PID');
tune.response('axes', subplot(2, 2, 3));

tune.PIChart('axes', subplot(2, 2, 2), 'dValue', 0.0);
tune.PIChart('axes', subplot(2, 2, 4), 'dValue', 0.03);

%% Plant dominated locking
gamma = 12e-3; % Hz 
gain = 1.0/(2.0*pi*gamma)*360; % degree/Hz
order = 8; bw = 20.35; 
plant = EquipmentControl.FeedBackOptimization.LTIUnit.LowPassFilter(1, gamma, 'gain', gain);
filter=EquipmentControl.FeedBackOptimization.LTIUnit.LowPassFilter(order, bw);
tune = EquipmentControl.FeedBackOptimization.PIDOptimizer(plant, filter);
figure; 
tune.setController1stOrderApprox();
tune.response('axes', subplot(2, 1, 1));

tune.setControllerOptimal('type', 'PID', 'tList', 0:0.001:0.25);
tune.response('axes', subplot(2, 1, 2), 'tList', 0:0.001:0.25);

figure
tune.PIChart('axes', axes, 'maxP', 0.5, 'maxI', 0.1);
